#include "FreshStartMcGeerWalker.h"

McGeerWalker::McGeerWalker(b2World* newWalkerWorld){
		walkerWorld = newWalkerWorld;
		radianToDegreesConst = -57.2957795f;

		groundAngle = 10.0f;
		groundFriction = 1.0f;
		footSize = 2.0f;
		footDensity = 0.05f;
		calfHeight = 5.0f;
		calfWidth = .5f;
		thighHeight = 5.0f;
		thighWidth = .75f;
		pelvisHeight = 0.0f;
		pelvisWidth = 0.0f;
		kneeUpperLimit = 0.0f / radianToDegreesConst;
		kneeLowerLimit = 10.0f / radianToDegreesConst;
		hipUpperLimit = -20.0f / radianToDegreesConst;
		hipLowerLimit = 20.0f / radianToDegreesConst;

		
		makeGround();
		makeBody(-20.0f);
		

}


void McGeerWalker::makeGround(){
	b2BodyDef bd;
	bd.angle = groundAngle / radianToDegreesConst;
	Ground = walkerWorld->CreateBody(&bd);
	b2PolygonDef sd;
	sd.density = 1.0f;
	sd.restitution = 0.0f;
	sd.friction = groundFriction;
	sd.SetAsBox(100.0f, 0.1f, b2Vec2(0.0f, -0.1f), 0.0f * b2_pi);
	Ground->CreateShape(&sd);
}


void McGeerWalker::makeBody(float xLocation){
	makeLeftFoot(xLocation, footSize, calfWidth, calfHeight);
	makeLeftThigh(thighWidth, thighHeight);
	makeRightFoot(xLocation, footSize, calfWidth, calfHeight);
	makeRightThigh(thighWidth, thighHeight);
	makeRightKnee();
	makeLeftKnee();
	makeHip();
}



void McGeerWalker::makeHip(){

	b2RevoluteJointDef hipDef;
	hipDef.Initialize(LThigh, RThigh , LThigh->GetWorldPoint(b2Vec2(0.0f, 0.0f + thighHeight)));
		
	hipDef.lowerAngle= hipLowerLimit;
	hipDef.upperAngle= hipUpperLimit;
	
	hipDef.enableLimit=true;
	hipDef.collideConnected = false;
	Hip = (b2RevoluteJoint*)walkerWorld->CreateJoint(&hipDef);
	
}

void McGeerWalker::makeLeftKnee(){

	b2RevoluteJointDef lKneeDef;
	lKneeDef.Initialize(LThigh, LFoot , LThigh->GetWorldPoint(b2Vec2(0.0f + thighWidth, 0.0f - thighHeight)));
		
	lKneeDef.lowerAngle = kneeLowerLimit;
	lKneeDef.upperAngle = kneeUpperLimit;
	lKneeDef.enableLimit=true;
	LKnee = (b2RevoluteJoint*)walkerWorld->CreateJoint(&lKneeDef);
}

void McGeerWalker::makeRightKnee(){

	b2RevoluteJointDef rKneeDef;
	//lKneeDef.Initialize(LThigh, LFoot, b2Vec2(LThigh->GetPosition().x + thighWidth, LThigh->GetPosition().y - thighHeight));
	rKneeDef.Initialize(RThigh, RFoot , RThigh->GetWorldPoint(b2Vec2(0.0f + thighWidth, 0.0f - thighHeight)));
		
	rKneeDef.lowerAngle= kneeLowerLimit;
	rKneeDef.upperAngle= kneeUpperLimit;
	rKneeDef.enableLimit=true;
	RKnee = (b2RevoluteJoint*)walkerWorld->CreateJoint(&rKneeDef);
}

void McGeerWalker::makeLeftThigh(float thighDefWidth, float thighDefHeight){
	b2BodyDef bd;
	bd.angle = 0.0f;
	LThigh = walkerWorld->CreateBody(&bd);
	
	b2PolygonDef sd;
	sd.density = 1.0f;
	sd.restitution = 0.0f;
	sd.friction = groundFriction;
	sd.SetAsBox(thighDefWidth, thighDefHeight);
	sd.filter.groupIndex = -1;
	LThigh->CreateShape(&sd);
	LThigh->SetXForm(b2Vec2(LFoot->GetPosition().x - (footSize/2) - thighDefWidth, LFoot->GetPosition().y + calfHeight + footSize + thighDefHeight), 0.0f);
	LThigh->SetMassFromShapes();
	
}

void McGeerWalker::makeLeftFoot(float xLocation, float footDefSize, float calfDefSize, float calfDefLength){
	//footType = McGeerWalker::LFoot;
	b2BodyDef bd;
	bd.angle = 0.0f;
	LFoot = walkerWorld->CreateBody(&bd);

	b2CircleDef sdFoot;
	sdFoot.density = footDensity;
	sdFoot.restitution = 0.0f;
	sdFoot.friction = groundFriction;
	sdFoot.radius = footDefSize / 2;
	sdFoot.localPosition.Set(0.0f, 0.0f);

	sdFoot.filter.groupIndex = -1;
	LFoot->CreateShape(&sdFoot);
	
	b2PolygonDef sdCalf;
	sdCalf.density = 1.0f;
	sdCalf.restitution = 0.0f;
	sdCalf.friction = groundFriction;
	sdCalf.SetAsBox(calfDefSize, calfDefLength, b2Vec2(calfDefSize + (sdFoot.localPosition.x - sdFoot.radius), sdFoot.localPosition.y + calfDefLength), 0.0f);

	sdCalf.filter.groupIndex = -1;
	LFoot->CreateShape(&sdCalf);
	LFoot->SetXForm( b2Vec2(xLocation, (tan(Ground->GetAngle()) * xLocation + sdFoot.radius + .1f)), 0.0f);
	LFoot->SetMassFromShapes();
	//Location coord = new Location(2, 4);

}
	
void McGeerWalker::makeRightThigh(float thighDefWidth, float thighDefHeight){
	b2BodyDef bd;
	bd.angle = 0.0f;
	RThigh = walkerWorld->CreateBody(&bd);
	
	b2PolygonDef sd;
	sd.density = 1.0f;
	sd.restitution = 0.0f;
	sd.friction = groundFriction;
	sd.SetAsBox(thighDefWidth, thighDefHeight);
	sd.filter.groupIndex = -1;
	RThigh->CreateShape(&sd);
	RThigh->SetXForm(b2Vec2(RFoot->GetPosition().x - (footSize/2) - thighDefWidth, RFoot->GetPosition().y + calfHeight + footSize + thighDefHeight), 0.0f);
	RThigh->SetMassFromShapes();
	
}

void McGeerWalker::makeRightFoot(float xLocation, float footDefSize, float calfDefSize, float calfDefLength){
	//footType = McGeerWalker::LFoot;
	b2BodyDef bd;
	bd.angle = 0.0f;
	RFoot = walkerWorld->CreateBody(&bd);

	b2CircleDef sdFoot;
	sdFoot.density = footDensity;
	sdFoot.restitution = 0.0f;
	sdFoot.friction = groundFriction;
	sdFoot.radius = footDefSize / 2;
	sdFoot.localPosition.Set(0.0f, 0.0f);
	sdFoot.filter.groupIndex = -1;

	RFoot->CreateShape(&sdFoot);
	
	b2PolygonDef sdCalf;
	sdCalf.density = 1.0f;
	sdCalf.restitution = 0.0f;
	sdCalf.friction = groundFriction;
	sdCalf.SetAsBox(calfDefSize, calfDefLength, b2Vec2(calfDefSize + (sdFoot.localPosition.x - sdFoot.radius), sdFoot.localPosition.y + calfDefLength), 0.0f);
	sdCalf.filter.groupIndex = -1;

	RFoot->CreateShape(&sdCalf);
	RFoot->SetXForm( b2Vec2(xLocation, (tan(Ground->GetAngle()) * xLocation + sdFoot.radius + .1f)), 0.0f);
	RFoot->SetMassFromShapes();

}
	
Location McGeerWalker::getNewCoordinates(Location currentPosition, int angle){
	angle = angle + 5;
	return currentPosition;
}